#include "motor_ctrl.h"
 
int32_t Motor_AK:: set_origin(uint8_t mode)
{
    return 0;
}
                                       
int32_t Motor_AK:: set_motor_pos(double pos)
{
#define POS_MIN       (-36000)
#define POS_MAX       (36000)
    if((pos >= POS_MIN) && (pos <= POS_MAX)) {
        this->pos = pos;
        return (int32_t)(pos * 10000);
    } else {
        this->pos = 0;
        return 0;
    }
}

int32_t Motor_AK:: set_motor_w(double w)
{
    /* 电气转速--->角速度
        x/64/21*60 = w
    */
#define W_MIN       (-100000.0f/64/21*60)
#define W_MAX       (100000.0f/64/21*60)
#define W_MIX_MIN   (-32768.0f/64/21*60)
#define W_MIX_MAX   (32768.0f/64/21*60)

    if(flag_mix == 0) {
        if((w >= W_MIN) && (w <= W_MAX)) {
            this->w = w;
            return (int32_t)(w*64*21/60);
        } else {
            this->w = 0;
            return 0;
        }
    } else {
        if((w >= W_MIX_MIN) && (w <= W_MIX_MAX)) {
            this->w = w;
            return (int32_t)(w*64*21/60);
        } else { 
            this->w = 0;
            return 0;
        }    
    }
} 
int32_t Motor_AK:: set_motor_t(double t)
{
#define T_MIN       (-60)
#define T_MAX       (60)
    if((t >= T_MIN) && (t <= T_MAX)) {
        this->t = t;
        return (int32_t)(t * 1000);
    } else {
        this->t = 0;
        return 0;
    }
} 
int32_t Motor_AK:: set_motor_acc(double acc)
{
#define ACC_MIN       (0)
#define ACC_MAX       (32767.0f/64/21*60)
    if((acc >= ACC_MIN) && (acc <= ACC_MAX)) {
        this->acc = acc;
        return (int32_t)(acc*64*21/60);
    } else {
        this->acc = acc;
        return 0;
    }
}

int32_t Motor_AK:: set_motor_kp(double kp)
{
    return 0;
}  
int32_t Motor_AK:: set_motor_kd(double kd)
{
    return 0;
}

void Motor_AK::set_cur_pos(uint8_t data[])
{
    int16_t pos = (data[0] << 8) | data[1];
    this->cur_pos = (pos*0.1f);
}

int32_t Motor_AK::set_motor_dataframe(uint8_t dataframe[], Motor_Mode mode, double value[], uint8_t len)
{
    int32_t data_len = 0;
    int32_t val;
    switch (mode) {
        case MOTOR_MODE_ENTER:
        case MOTOR_MODE_EXIT:
            data_len = 0;
            break;
        case MOTOR_MODE_ORIGIN:
            data_len = 1;
            dataframe[0] = 0;         //0-临时 1-永久 2-恢复默认
            break;
        case MOTOR_MODE_W:  //value为角速度
            data_len = 4;
            if(1 > len) {
                cout << "MOTOR_TYPE_AK MOTOR_MODE_W len error!" << endl;
                return -1;
            }
            val = set_motor_w(value[0]);
            dataframe[0] =  (val>>24) & 0xff;
            dataframe[1] =  (val>>16) & 0xff;
            dataframe[2] =  (val>>8) & 0xff;
            dataframe[3] =  val & 0xff;
            break;
        case MOTOR_MODE_POS:    //value为位置
            data_len = 4;
            if(1 > len) {
                cout << "MOTOR_TYPE_AK MOTOR_MODE_POS len error!" << endl;
                return -1;
            }
            val = set_motor_pos(value[0]);
            dataframe[0] =  (val>>24) & 0xff;
            dataframe[1] =  (val>>16) & 0xff;
            dataframe[2] =  (val>>8) & 0xff;
            dataframe[3] =  val & 0xff;
            break;
        case MOTOR_MODE_T:  //value为扭矩
            data_len = 4;
            if(1 > len) {
                cout << "MOTOR_TYPE_AK MOTOR_MODE_T len error!" << endl;
                return -1;
            }
            val = set_motor_t(value[0]);
            dataframe[0] =  (val>>24) & 0xff;
            dataframe[1] =  (val>>16) & 0xff;
            dataframe[2] =  (val>>8) & 0xff;
            dataframe[3] =  val & 0xff;
            break;   
        case MOTOR_MODE_MIX:    /* value 依次是 位置 速度 加速度 */
            data_len = 8;
            flag_mix = 1;
            if(3 > len) {
                cout << "MOTOR_TYPE_AK MOTOR_MODE_MIX len error!" << endl;
                return -1;
            }
            val = set_motor_pos(value[0]);
            dataframe[0] =  (val>>24) & 0xff;
            dataframe[1] =  (val>>16) & 0xff;
            dataframe[2] =  (val>>8) & 0xff;
            dataframe[3] =  val & 0xff;

            val = set_motor_w(value[1]);
            dataframe[4] =  (val>>8) & 0xff;
            dataframe[5] =  val & 0xff;

            val = set_motor_acc(value[2]);
            dataframe[6] =  (val>>8) & 0xff;
            dataframe[7] =  val & 0xff;
            flag_mix = 0;
            break; 
        default:
            break;
    }
#if 0
    printf("dataframe=");
    for(uint8_t i =0;i<8;i++) {
        printf("0x%02x ",dataframe[i]);
    }
    printf("\n");
#endif
    return data_len;
}

uint8_t Motor_AK::get_motor_mode_map(Motor_Mode mode)
{
    uint8_t ret=0;
    switch(mode) {
        case MOTOR_MODE_ENTER:
        case MOTOR_MODE_EXIT:
            ret = 0;
            break;
        case MOTOR_MODE_ORIGIN:
            ret = 5;
            break; 
        case MOTOR_MODE_W:
            ret = 3;
            break;
        case MOTOR_MODE_POS:
            ret = 4;
            break;
        case MOTOR_MODE_T:
            ret = 1;
            break;
        case MOTOR_MODE_MIX:
            ret = 6;
            break;
        default:
            break;
    }
    return ret;
} 